import Interface.BluetoothNXT;
import lejos.nxt.*;
import lejos.robotics.navigation.TachoPilot;




/**Procedurale LineFollower **/
public class LineFollower {
	private static final boolean VERBOSE = false;

	public static void main(String [] args) throws InterruptedException{		
		final float leftWheelDiameter = 56; 
		final float rightWheelDiameter = 56;
		final float trackWidth = 164;
		Motor leftMotor = Motor.B;
		Motor rightMotor = Motor.C;
		
		TachoPilot pilot = new TachoPilot(leftWheelDiameter, rightWheelDiameter, trackWidth, leftMotor, rightMotor, false);
		LightSensor lightSensor = new LightSensor(SensorPort.S1);
		LCD.clearDisplay();
		LCD.drawString("Callibrate white", 0, 0);
		lightSensor.calibrateLow();		
		Thread.sleep(3000);	
		LCD.drawString("Calibrate black", 0, 1);
		lightSensor.calibrateHigh();
		Thread.sleep(3000); 
		LCD.drawString("Calibrate color to follow", 0, 2);
		int toFollow = lightSensor.getLightValue();
		Thread.sleep(3000); 
		LCD.drawString("Calibrate floor", 0, 3);
		int floor = lightSensor.getLightValue();		
		Thread.sleep(3000);	LCD.clearDisplay();
		
		//connect to bluetooth
		LCD.drawString("Connect Bluetooth?", 0, 1);
		int button = Button.waitForPress();
		if (button == Button.ID_ENTER)
		{		
			BluetoothNXT test = new BluetoothNXT(pilot,lightSensor, new UltrasonicSensor(SensorPort.S2));
			test.start();
		}
		
		LCD.drawString("Start in 2", 0,0); Thread.sleep(1000); LCD.drawString("Start in 1", 0,1); Thread.sleep(1000);
		
	
		int offset = Math.abs(toFollow -  floor)/2;
		int distancePerLoop = 20;		
		
		if (VERBOSE){LCD.clearDisplay();	LCD.drawString("Bezig met zoeken naar lijn", 0,0);Thread.sleep(500);};
		int angle = 6;
		while(!onLine(lightSensor, toFollow, offset, "1e kr lijn")){ //Rij voorwaarts tot je de lijn vind
			pilot.travel(distancePerLoop);
		}
		
		if (VERBOSE){LCD.clearDisplay();	LCD.drawString("Lijn gevonden!", 0,0);Thread.sleep(500);};
		while(true){
			forwardTillLineLost(pilot, lightSensor, toFollow, offset,distancePerLoop);
			
			if (VERBOSE){LCD.clearDisplay();	LCD.drawString("Lijn Kwijt! Zoeken...", 0,0);Thread.sleep(500);};
			lookForLine(pilot, lightSensor, toFollow, offset, angle);
			if (VERBOSE){LCD.clearDisplay();	LCD.drawString("Lijn teruggevonden!", 0,0);Thread.sleep(500);};
		}	
			
	}

	private static void forwardTillLineLost(TachoPilot pilot,LightSensor lightSensor, int toFollow, int offset,	int distancePerLoop) throws InterruptedException {
		if (VERBOSE){LCD.clearDisplay();	LCD.drawString("FW tot kwijt", 0,0);Thread.sleep(500);};
		while(onLine(lightSensor, toFollow, offset, "FW tot weg")){ //Rij vooruit zolang je op de lijn bent
			pilot.travel(distancePerLoop);				
		}
	}

	private static void lookForLine(TachoPilot pilot, LightSensor lightSensor, int toFollow, int offset, int angle) throws InterruptedException {
		while(!onLine(lightSensor, toFollow, offset, "Zoek Lijn wh")){
			pilot.rotate(angle);
			if(onLine(lightSensor, toFollow, offset, "Zoek lijn r")) break;
			pilot.rotate(-2*angle);			
			if(onLine(lightSensor, toFollow, offset, "Zoek lijn l")) break;
			pilot.rotate(angle);
			angle+=8;
		}		
	}
	
	private static boolean onLine(LightSensor sensor, int toFollow, int offset, String currentTask) throws InterruptedException{	
		LCD.clearDisplay();
		LCD.drawString("" + currentTask, 0, 0);
		LCD.drawString("waarde: " + sensor.getLightValue(), 0, 1);
		LCD.drawString("toFollow: " + toFollow, 0, 2);
		LCD.drawString("toF+off: " + (toFollow + offset), 0, 3);
		LCD.drawString("toF-off: " + (toFollow - offset), 0, 4);
		
		if (VERBOSE){Thread.sleep(200);};
		return (sensor.getLightValue() < toFollow + offset) && (sensor.getLightValue() > toFollow - offset);
	}
}
